package org.opentcs.kcvehicle.communication.kc.udp.Service;

import org.opentcs.kcvehicle.communication.kc.udp.agv.param.AgvEvent;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.navigation.Action;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.navigation.ActionSet;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kcvehicle.communication.kc.udp.io.UDPClient;

public class ActImmediately extends BaseCommand{

  private static Integer actionId = 1;

  /**
   * decs: 立即动作指令
   * 指令：0xB2
   * author: caixiang
   * date: 2025/1/17 16:25
   */
  private static AgvEvent actionNow(float execuHeight, byte modeOfMovement) {
    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ACT_IMMEDIATELY);
    //TODO 构建

    //构建Action指令
    Action actionLiftUp = new Action(
        ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节
        (byte) 0x02,   //只能执行当前动作
        actionId,     //动作ID
        ActionSet.fork0x12_paramsize,
        ActionSet.fork0x12(
            execuHeight,   //升降高度
            modeOfMovement,  //1上升  2下降
            (byte)0x01   //01 开始任务
        )
    );

    actionId++;
    agvEvent.setBody(actionLiftUp.toBytes());
    return agvEvent;
  }

  private static AgvEvent resetCmd() {
    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ACT_IMMEDIATELY);
    //重置 空指令
    Action actionReset = new Action(
        ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节
        (byte) 0x02,   //只能执行当前动作
        actionId,     //actionID
        ActionSet.fork0x12_paramsize,
        ActionSet.fork0x12(
            0.1f,   //升降高度
            (byte) 0x01,  //1上升  2下降
            (byte)0x00   //00 无任务
        )
    );
    agvEvent.setBody(actionReset.toBytes());

    actionId++;
    agvEvent.setBody(actionReset.toBytes());
    return agvEvent;
  }


  public static void command(float execuHeight, byte modeOfMovement){
    //0xB2(立即动作指令)
    AgvEvent agvEvent = actionNow(execuHeight, modeOfMovement);
    printInfo(agvEvent);
    RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
    if(rcv.isOk()){
      System.out.println("0xB2 forklift ok");
    }else {
      System.out.println();
      System.out.println("0xB2 forklift fail");
      System.out.println("received transationId : "+ "isok:"+rcv.isOk());
    }
  }

  public static void reset(){
    //0xB2(立即动作指令)
    AgvEvent agvEvent = resetCmd();
    printInfo(agvEvent);
    RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
    if(rcv.isOk()){
      System.out.println("-0xB2 reset ok");
    }else {
      System.out.println();
      System.out.println("-0xB2 reset fail");
      System.out.println("-received transationId : "+ "isok:"+rcv.isOk());
    }
  }

  /**
   * 读取动作状态
   */
  public static void readActionState() {

    try {
      Thread.sleep(100);

      //todo 读取动作执行状态
      byte[] value = ReadValue.command();
      int execStatus = value[0];

      System.out.println();
      System.out.println("-execStatus:" + execStatus);

      while (execStatus == 2) {
        Thread.sleep(500);
        byte[] value1 = ReadValue.command();
        execStatus = value1[0];
      }

      Thread.sleep(100);
    } catch (Exception e) {
      throw new RuntimeException("readActionState:" + e);
    }
  }

  public static void allotsOperation(float height, byte modeOfMovement) {

    //读取动作执行状态
    readActionState();
    //重置
    ActImmediately.reset();
    //读取动作执行状态
    readActionState();
    //执行叉齿指令
    ActImmediately.command(height, modeOfMovement);
    //读取动作执行状态
    readActionState();
    //重置
    ActImmediately.reset();
    //读取重置状态
    readActionState();
  }
}
